package utils;

import java.lang.Math;
import java.nio.DoubleBuffer;
import java.nio.FloatBuffer;
import java.util.Random;


public class Vec {
	public double x, y, z;

	public static Vec tmp = new Vec();
	public static Vec tmp2 = new Vec();

	public final double x() {
		return x;
	}

	public final double y() {
		return y;
	}

	public final double z() {
		return z;
	}
	
	public void Floatify() {
		x = (float)x;
		y = (float)y;
		z = (float)z;
	}

	public void Intitfy() {
		x = (int)x;
		y = (int)y;
		z = (int)z;
	}

	public static double[] pm = new double[16];
	
	
	public void print(String n) {
		System.out.println(n + ": [" + x +", " + y + ", " +z+"]");
	}
	
	public void floor() {
		x = Math.floor(x);
		y = Math.floor(y);
		z = Math.floor(z);
	}

	public final void fromRad(double r, double t) {
		x = (double) (r * Math.sin(t));
		y = (double) (r * Math.cos(t));
		z = 0f;
	}

	public String toString() {
		return "(" + x + ", " + y + ", " + z + ")";
	}

	public final void fromSpherical(double r, double theta, double phi) {
		x = (double) (r * Math.cos(phi) * Math.sin(theta));
		y = (double) (r * Math.sin(phi) * Math.sin(theta));
		z = (double) (r * Math.cos(theta));
	}

	public void Linear(final Vec a, final Vec b, final double t) {
		double t2 = 1f - t;
		x = a.x() * t + b.x() * t2;
		y = a.y() * t + b.y() * t2;
		z = a.z() * t + b.z() * t2;

	}

	public void randomize(Random rand) {
		x = (rand.nextDouble() - 0.5f) * x;
		y = (rand.nextDouble() - 0.5f) * y;
		z = (rand.nextDouble() - 0.5f) * z;
	}

	public void randomizePositive(Random rand) {
		x = (rand.nextDouble()) * x;
		y = (rand.nextDouble()) * y;
		z = (rand.nextDouble()) * z;
	}

	public void randomizeXY(Random rand) {
		x = (rand.nextDouble() - 0.5f) * x;
		y = (rand.nextDouble() - 0.5f) * y;
	}

	public void randomizeXZ(Random rand) {
		x = (rand.nextDouble() - 0.5f) * x;
		z = (rand.nextDouble() - 0.5f) * z;
	}

	public double get(final int i) {
		if (i == 0)
			return x;
		if (i == 1)
			return y;
		return z;
	}

	private final double minmax(final double a, final double min,
			final double max) {
		if (a > max)
			return max;
		if (a < min)
			return min;
		return a;
	}

	public void interpolate(final Vec a, final Vec b, double val) {
		double t1 = minmax(val, 0, 1);
		double t2 = minmax(1f - val, 0, 1);

		x(a.x() * (t2) + b.x() * t1);
		y(a.y() * (t2) + b.y() * t1);
		z(a.z() * (t2) + b.z() * t1);
	}

	static public double PointDistanceToRay(final Vec origo, final Vec ray,
			final Vec p, Vec isec) {
		tmp.sub(p, origo);
		double l = tmp.dot(ray);
		tmp.set(origo);
		tmp.addDirect(ray, l);

		if (isec != null)
			isec.set(tmp);

		tmp.subDirect(p);

		return tmp.len();
		// return p.cross(ray).len() / ray.len();
	}

	public final void z(double v) {
		z = v;
	}

	public final void x(double v) {
		x = v;
	}

	public final void y(double v) {
		y = v;
	}

	public Vec() {
		this.x = 0;
		this.y = 0;
		this.z = 0;
	}

	public Vec(final double x) {
		this.x = x;
		this.y = x;
		this.z = x;
	}

	public Vec(final double x, final double y, final double z) {
		this.x = x;
		this.y = y;
		this.z = z;
	}

	public Vec(Vec tmp3) {
		// TODO Auto-generated constructor stub
		set(tmp3);
	}

	public final void set(final double x, final double y, final double z) {
		this.x = x;
		this.y = y;
		this.z = z;
	}

	public final void set(final Vec o) {
		this.x = o.x;
		this.y = o.y;
		this.z = o.z;
	}

	public final void set(final FloatBuffer buf, int idx) {
		this.x = buf.get(idx+0);
		this.y = buf.get(idx+1);
		this.z = buf.get(idx+2);
	}

	public final void toFloatBuffer(final FloatBuffer buf, int idx) {
		buf.put(idx+0, (float)x);
		buf.put(idx+1, (float)y);
		buf.put(idx+2, (float)z);
	}

	public final void set(final DoubleBuffer buf, int idx) {
		this.x = buf.get(idx+0);
		this.y = buf.get(idx+1);
		this.z = buf.get(idx+2);
	}


	
	
	public final void cross(final Vec a, final Vec b) {
		set(b.y * a.z - b.z * a.y, b.z * a.x - b.x * a.z, b.x * a.y - b.y * a.x);
	}

	public final void interpolate(final Vec o, final double t) {
		double t2 = 1 - t;
		this.mulDirect(t);
		tmp.addDirect(o, t2);
	}

	public final void maxSpeed(double m, double dt) {
		if (len2() > m * m) {
			tmp.set(this);
			tmp.normDirect();
			tmp.mulDirect(m);
			interpolate(tmp, 0.98f - dt);
		}

	}

	public final double dot(final Vec o) {
		return x * o.x + y * o.y + z * o.z;
	}

	public final void addDirect(final Vec o) {
		x += o.x;
		y += o.y;
		z += o.z;
	}

	public final void addDirect(final Vec o, final double s) {
		x += o.x * s;
		y += o.y * s;
		z += o.z * s;
	}

	public final void subDirect(final Vec o, final double s) {
		x -= o.x * s;
		y -= o.y * s;
		z -= o.z * s;
	}

	public final void subDirect(final Vec o) {
		x -= o.x;
		y -= o.y;
		z -= o.z;
	}

	/*
	 * public final Vec sub(final Vec o) { return new Vec(x-o.x, y-o.y, z-o.z);
	 * }
	 */
	public final void sub(final Vec a, final Vec b) {
		set(a.x() - b.x(), a.y() - b.y(), a.z() - b.z());
	}

	public final void add(final Vec a, final Vec b) {
		set(a.x() + b.x(), a.y() + b.y(), a.z() + b.z());
	}

	/*
	 * public final Vec mul(final double f) { return new Vec(x*f, y*f, z*f); }
	 */
	public final void mulDirect(final double f) {
		x *= f;
		y *= f;
		z *= f;
	}

	public final void mulDirect(final Vec other, final double f) {
		x *= other.x() * f;
		y *= other.y() * f;
		z *= other.z() * f;
	}

	/*
	 * public final Vec norm() { return new Vec(x,y,z).mul(1.0f/len()); }
	 */
	public final void normDirect() {
		double l = len();
		if (l == 0.0)
			l = 1.0;
		x /= l;
		y /= l;
		z /= l;
	}

	public final double len() {
		return Math.sqrt(x * x + y * y + z * z);
	}

	public final double len2() {
		return (x * x + y * y + z * z);
	}

	public final void glMatMul(Vec res, double[] pm) {

		res.x = pm[0] * x + pm[1] * y + pm[2] * z;

		res.y = pm[4] * x + pm[5] * y + pm[6] * z;

		res.z = pm[8] * x + pm[9] * y + pm[10] * z;
	}

	public final void glMatMul(Vec res, float[] pm) {

		res.x = pm[0] * x + pm[1] * y + pm[2] * z;

		res.y = pm[4] * x + pm[5] * y + pm[6] * z;

		res.z = pm[8] * x + pm[9] * y + pm[10] * z;
	}

	
	public final void matMul(Vec res, Matrix3D pm) {

		res.x = pm.get(0, 0) * x + pm.get(1, 0) * y + pm.get(2, 0) * z;

		res.y = pm.get(0, 1) * x + pm.get(1, 1) * y + pm.get(2, 1) * z;

		res.z = pm.get(0, 2) * x + pm.get(1, 2) * y + pm.get(2, 2) * z;
	}

	public final void periodic2D(double W, double H) {
		double f = 1.1f;
		double f2 = f * 0.99f;
		if (x > f * W)
			x = (-f2 * W);
		if (y > f * H)
			y = (-f2 * H);
		if (x < -f * W)
			x = (f2 * W);
		if (y < -f * H)
			y = (f2 * H);

	}

	static public boolean intersectray_triangle(final Vec p1, final Vec p2,
			final Vec p3, final Vec isp) {

		// if ((isp-origin).Length()<len)
		{
			Vec v1 = new Vec();
			Vec v2 = new Vec();
			Vec v3 = new Vec();

			// System.out.println(p1.x() + " , " + isp.x());

			v1.sub(isp, p1);
			v1.normDirect();
			v1.mulDirect(1f);

			v2.sub(isp, p2);
			v2.normDirect();
			v2.mulDirect(1f);

			v3.sub(isp, p3);
			v3.normDirect();
			v3.mulDirect(1f);

			double dAngle = (Math.acos(v1.dot(v2)) + Math.acos(v2.dot(v3)) + Math
					.acos(v3.dot(v1)));
			dAngle = Math.abs(dAngle - 2.0 * 3.14159265);
			if (dAngle < 0.1)
				return true;
		}

		return false;
	}

	static public Vec RayIntersectPlane(Vec planeOrigin, Vec planeNormal,
			Vec rayOrigin, Vec rayVector) {

		Vec isp = new Vec();
		Vec SuperPos = new Vec();
		Vec Pos1 = new Vec();
		Vec Pos2 = new Vec();
		Vec p3 = new Vec();

		SuperPos.set(rayVector);
		SuperPos.normDirect();

		SuperPos.mulDirect(1000.0f);
		SuperPos.addDirect(rayOrigin);

		Pos1.sub(rayOrigin, planeOrigin);
		Pos2.sub(SuperPos, planeOrigin);
		p3.sub(SuperPos, rayOrigin);

		double sc1 = Pos1.dot(planeNormal);
		double sc2 = Pos2.dot(planeNormal);
		if ((sc1 - sc2) == 0)
			return isp;

		double Scale = (sc1 / (sc1 - sc2));
		isp.set(rayOrigin);
		isp.addDirect(p3, Scale);
		return isp;

	}

	public void TreeInterpolate(Vec v1, Vec v2, Vec v3, final double val) {
		// val is between 0 and 1, 0.5 = v2
		double sc1, sc2, sc0;
		if (val <= 0.5) {
			sc2 = 0.0;
			sc1 = 2.0 * val;
			sc0 = 1.0 - sc1;
		} else {
			double v = val - 0.5;
			sc1 = 1.0 - (2 * v);
			sc2 = 1.0 - sc1;
			sc0 = 0.0;
		}

		set(0, 0, 0);
		addDirect(v1, sc0);
		addDirect(v2, sc1);
		addDirect(v3, sc2);

	}


}
